/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright
*
*********************************************************************/

// Define DEBUG_OUTPUT to enable PRINTLN_DEBUG output when not using ROS.
// ROS debug level output is toggled at runtime using rxconsole.
//#define DEBUG_OUTPUT

// ================================================================== includes

#include <iostream>

using namespace std;

#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdint.h>
#include <stdio.h>
#include <sys/stat.h>
#include <sys/types.h>

#include <linux/joystick.h>

#include "Joystick.h"

using namespace std;
using namespace ost;

// =================================================================== defines

#define MAX_ANALOG_VALUE 32767
#define FORWARD_THRUST_LIMIT 100
#define OTHER_THRUST_LIMIT 100

#define VELOCITY_SCALE_X 0.5
#define VELOCITY_SCALE_Y 0.2
#define VELOCITY_SCALE_Z 0.2
#define VELOCITY_SCALE_YAW 45
#define VELOCITY_SCALE_PITCH 30

static float thrust_test_val = OTHER_THRUST_LIMIT;
static char thrust_test_axis = 'z';

static float thrust_test_val2 = -(OTHER_THRUST_LIMIT);
static char thrust_test_axis2 = 'z';

// =================================================================== methods
// ---------------------------------------------------------------------------
bool Joystick::init(int argc, char * argv[])
{
	PRINTLN_DEBUG("init entered");

	if (argc == 3) {
		thrust_test_axis = argv[1][0];
		thrust_test_val = atof(argv[2]);
		thrust_test_axis2 = thrust_test_axis;
		thrust_test_val = -thrust_test_val;

		std::cout << "Both right shoulder button axis " << thrust_test_axis << ", value " << thrust_test_val << std::endl;
	}
	else if (argc == 5) {
		thrust_test_axis = argv[1][0];
		thrust_test_val = atof(argv[2]);
		thrust_test_axis2 = argv[3][0];
		thrust_test_val2 = atof(argv[4]);

		std::cout << "Bottom-right shoulder button axis " << thrust_test_axis << ", value " << thrust_test_val << std::endl;
		std::cout << "Top-right shoulder button    axis " << thrust_test_axis2 << ", value " << thrust_test_val2 << std::endl;
	}

	directMode = false;
	invertPitch = false;
	overrideForceMode = false;
	velocityMode = false;
	m_haveNav = false;
	m_waypointActive = false;

	// Check args.
	for (int i=1; i<argc; ++i)
	{
		if (strcmp(argv[i], "-aardvark") == 0)
		{
			cout << "The flag '-aardvark' has been replaced; please use '--direct' instead." << endl;
			return false;
		}
		else if (strcmp(argv[i], "--direct") == 0)
		{
			cout << "Enabled direct to thrusters mode (not via pilot)." << endl;
			directMode = true;
		}
		else if (strcmp(argv[i], "--override") == 0)
		{
			cout << "Enabled override force sending mode." << endl;
			overrideForceMode = true;
		}
		else if (strcmp(argv[i], "--velocity") == 0)
		{
			cout << "Enabled velocity sending mode." << endl;
			velocityMode = true;
		}
		else if (strcmp(argv[i], "--invert-pitch") == 0)
		{
			cout << "Enabled pitch stick inversion (stick up brings nose up)." << endl;
			invertPitch = true;
		}
		else if ((stricmp(argv[i], "-p") == 0) && (i+1 < argc))
		{
			//cout << "Using joystick device: " << argv[i+1] << endl;
			deviceName = argv[i+1];
		}
	}

	if(deviceName.length() == 0)
	{
		deviceName = DEFAULT_DEVICE_PATH;
		PRINTLN_INFO("Using DEFAULT Device: " << deviceName);
	}
	else
	{
		PRINTLN_INFO("Using custom Device: " << deviceName);
	}
	//device = new string("/dev/input/js1");

	sendPeriod = 100;
	sendTimer.setTimer(1);

	memset(&pad, 0, sizeof(pad));
	pad.fd = open(deviceName.c_str(), O_RDONLY);
	if (pad.fd <= 0)
	{
		::close(pad.fd);
		cerr << "Failed to open joystick - check it is attached." << endl;
		return false;
	}

	cout << "Joystick opened ok." << endl;

	// Get pad info ...
	ioctl(pad.fd, JSIOCGAXES, &pad.axisCount);
	ioctl(pad.fd, JSIOCGBUTTONS, &pad.buttonCount);
	ioctl(pad.fd, JSIOCGVERSION, &pad.version);
	ioctl(pad.fd, JSIOCGNAME(80), &pad.devName);
	fcntl(pad.fd, F_SETFL, O_NONBLOCK);

	cout << "axis : " << (int)pad.axisCount << endl;
	cout << "buttons : " << (int)pad.buttonCount << endl;
	cout << "version : " << pad.version << endl;
	cout << "name : " << pad.devName << endl;

	// set default values
	pad.changed = false;
	for (int i=0;i<pad.axisCount;i++) pad.aPos[i]=0;
	for (int i=0;i<pad.buttonCount;i++) pad.bPos[i]=0;

	lastValuesNonZero = true;

 	// Published topics.
	if (directMode) {
		m_rosVehicleForcePub = m_rosHandle->advertise<geometry_msgs::WrenchStamped>("pilot/force_req", 1);
	}
	else if (overrideForceMode) {
		m_rosVehicleForcePub = m_rosHandle->advertise<geometry_msgs::WrenchStamped>("pilot/override_force_req", 1);
	}
	else if (velocityMode) {
		m_rosVehicleVelPub = m_rosHandle->advertise<auv_msgs::BodyVelocityReq>("pilot/body_velocity_req", 1);
	}
	else {
		m_rosVehicleForcePub = m_rosHandle->advertise<geometry_msgs::WrenchStamped>("pilot/manual_force_req", 1);
	}

	m_rosNavStsSub = m_rosHandle->subscribe<auv_msgs::NavSts>("nav/nav_sts",1, &Joystick::processNavSts, this, m_rosHints);
	m_rosWorldWaypointPub = m_rosHandle->advertise<auv_msgs::WorldWaypointReq>("pilot/world_waypoint_req",1);

	// Random number for waypoint id

	m_idWaypoint=rand();

	PRINTLN_DEBUG("initialisation complete");

	return true;
}


// ---------------------------------------------------------------------------
void Joystick::doWork()
{
	if(sendTimer.getTimer() == 0)
	{
		sendTimer.setTimer(sendPeriod);

		readLatestState();

		//printPadState();

		float x, y, z, roll, pitch, yaw;
		x = scaleStick(-pad.aPos[1], FORWARD_THRUST_LIMIT);
		y = scaleStick(pad.aPos[0], OTHER_THRUST_LIMIT);
		forceSend = pad.bPos[6];

		roll = 0;
		bool pitchMode;
		if (pad.bPos[4]) {
			z = 0;
			pitch = scaleStick(pad.aPos[3], OTHER_THRUST_LIMIT);
			if (invertPitch) pitch = -pitch;
			pitchMode = true;
		}
		else {
            // moved dive to the buttons 5 and 7
			z = scaleStick(pad.aPos[3], OTHER_THRUST_LIMIT);
            //PRINTLN_DEBUG("value: " << pad.aPos[3]);
			pitch = 0;
			pitchMode = false;
		}

		yaw = scaleStick(pad.aPos[2], OTHER_THRUST_LIMIT);

		bool surfacing = pad.bPos[5];
		bool diving = pad.bPos[7];

		float test_thrust = 0;
		if (diving) {
			test_thrust = thrust_test_val;
			switch (thrust_test_axis)
			{
			case 'x': x = test_thrust; break;
			case 'y': y = test_thrust; break;
			case 'z': z = test_thrust; break;
			case 'P': pitch = test_thrust; break;
			case 'Y': yaw = test_thrust; break;
			}
		}
		else if (surfacing) {
			test_thrust = thrust_test_val2;
			switch (thrust_test_axis2)
			{
			case 'x': x = test_thrust; break;
			case 'y': y = test_thrust; break;
			case 'z': z = test_thrust; break;
			case 'P': pitch = test_thrust; break;
			case 'Y': yaw = test_thrust; break;
			}
		}

		//Set or unset waypoint (Button 4 must be pressed as well)
		if(pad.bPos[4] && pad.bPos[0]) {
			m_waypointActive =true;
			if(m_haveNav){
				ROS_INFO("Setting waypoint");
				setWaypoint();
			}
			else{
				ROS_INFO("Waiting for nav!");			
			}
		}
		else if(pad.bPos[4] && pad.bPos[1]) {
			m_waypointActive =false;
			unsetWaypoint();
			ROS_INFO("Releasing waypoint");
		}

		if(m_waypointActive) sendWaypoint();
		
//		if (surfacing) {
//			z = scaleStick(-32767, OTHER_THRUST_LIMIT);
//		}
//		else if (diving) {
//			z = scaleStick(32767, OTHER_THRUST_LIMIT);
//		}

		bool sending = true;

		if (directMode)
		{
			cout << "Direct mode, sending msg regardless of joystick forces." << endl;
		}
		else if (forceSend)
		{
			cout << "Force send button held - sending msg even if zero force." << endl;
			lastValuesNonZero = true;
		}
		else if (x != 0 || y != 0 || roll != 0 || pitch != 0 || yaw != 0 || z != 0)
		{
			cout << "Joystick forces non-zero, sending msg." << endl;
			lastValuesNonZero = true;
		}
		else if (lastValuesNonZero)
		{
			cout << "Sending last zero msg, then stopping." << endl;
			lastValuesNonZero = false;
		}
		else {
			sending = false;
		}


		if (sending) {
			if (pitchMode) {
				if (invertPitch) cout << "Right joystick inverted pitch mode. ";
				else cout << "Right joystick pitch mode. ";
			}
			else cout << "Right joystick dive mode. ";

			if (surfacing) {
				cout << "Surface button held - max up thrust.";
			}
			else if (diving) {
				cout << "Dive button held - max down thrust.";
			}

			cout << endl;

			if (velocityMode) {
				sendVelocityMsg(x, y, z, roll, pitch, yaw);
			}
			else {
				sendForceMsg(x, y, z, roll, pitch, yaw);
			}
		}
	}
}

// ---------------------------------------------------------------------------
float Joystick::scaleStick(int value, float limit)
{
	return value * limit / MAX_ANALOG_VALUE;
}

// ---------------------------------------------------------------------------
bool Joystick::readLatestState()
{
	pad.changed = false;
	int result;
	while ((result = read(pad.fd, &pad.ev, sizeof(pad.ev))) > 0)
	{
		switch (pad.ev.type)
		{
		case JS_EVENT_INIT:
		case JS_EVENT_INIT | JS_EVENT_AXIS:
		case JS_EVENT_INIT | JS_EVENT_BUTTON:
		break;
		case JS_EVENT_AXIS:
		pad.aPos[pad.ev.number] = pad.ev.value;
		pad.changed = true;
		break;
		case JS_EVENT_BUTTON:
		pad.bPos[pad.ev.number] = pad.ev.value;
		pad.changed = true;
		break;
		default:
		printf("Other event ? %d\nnumber=%d\nvalue=%d\n",
		pad.ev.type,pad.ev.number, pad.ev.value);
		break;
		}
	}

	if (errno != EAGAIN) {
		cerr << "Aborting on joystick read error: " << errno << endl;
		requestRestart();
	}

	return pad.changed;
}

// ---------------------------------------------------------------------------
void Joystick::printPadState()
{
	cout << "----------------------------------------------" << endl;
	cout << "pad.changed: " << pad.changed << endl;
	cout << "axis : " << pad.axisCount << endl;
	cout << "buttons : " << pad.buttonCount << endl;
	cout << "version : " <<  pad.version << endl;
	cout << "name : " << pad.devName << endl;
	cout << "----------------------------------------------" << endl;
	cout << "last ev time : " << pad.ev.time << endl;

	for (int i=0;i<pad.axisCount;i++) printf(" Axis %2d |",i);
	printf("\n");
	for (int i=0;i<pad.axisCount;i++) printf(" %7d |",pad.aPos[i]);
	printf("\n\n");
	for (int i=0;i<pad.buttonCount;i++) printf(" Btn.%2d |",i);
	printf("\n");
	for (int i=0;i<pad.buttonCount;i++) printf("     %2d |",pad.bPos[i]);
	printf("\n");
}


// ---------------------------------------------------------------------------
void Joystick::sendForceMsg(float x, float y, float z, float roll, float pitch, float yaw)
{
	// The ROS messages are the same, but topic name has already been configured appropriately in init().
	geometry_msgs::WrenchStamped rosMsg;
	rosMsg.header.stamp = ros::Time::now();

	// ROS message uses +/- 1 instead of +/- 100%, so scale down.
	rosMsg.wrench.force.x = x * 0.01;
	rosMsg.wrench.force.y = y * 0.01;
	rosMsg.wrench.force.z = z * 0.01;
	rosMsg.wrench.torque.x = roll * 0.01;
	rosMsg.wrench.torque.y = pitch * 0.01;
	rosMsg.wrench.torque.z = yaw * 0.01;

	double epsilon = 0.01;

	if (overrideForceMode && !forceSend) {
		// If any axis value is close to zero, set the message force for
		// that axis to NAN, to tell the pilot not to override it.

		if (abs(rosMsg.wrench.force.x) < epsilon) rosMsg.wrench.force.x = NAN;
		if (abs(rosMsg.wrench.force.y) < epsilon) rosMsg.wrench.force.y = NAN;
		if (abs(rosMsg.wrench.force.z) < epsilon) rosMsg.wrench.force.z = NAN;
		if (abs(rosMsg.wrench.torque.x) < epsilon) rosMsg.wrench.torque.x = NAN;
		if (abs(rosMsg.wrench.torque.y) < epsilon) rosMsg.wrench.torque.y = NAN;
		if (abs(rosMsg.wrench.torque.z) < epsilon) rosMsg.wrench.torque.z = NAN;
	}

	m_rosVehicleForcePub.publish(rosMsg);

	if (directMode) {
		cout << "Sending force message direct to motor controller." << endl;
	}
	else if (overrideForceMode) {
		cout << "Sending override force request to pilot." << endl;
	}
	else {
		cout << "Sending manual force request to pilot." << endl;
	}

	const int p = 2;
	const int w = 7;
	cout << endl
			<< "X:    " << fixed << setprecision(p) << setw(w) << x << ", "
			<< "Y:    " << fixed << setprecision(p) << setw(w) << y << ", "
			<< "Z:    " << fixed << setprecision(p) << setw(w) << z << endl
			<< "Roll: " << fixed << setprecision(p) << setw(w) << roll << ", "
			<< "Pitch:" << fixed << setprecision(p) << setw(w) << pitch << ", "
			<< "Yaw:  " << fixed << setprecision(p) << setw(w) << yaw << endl;

	cout << "--------------- time " << currentTimeString() << " ---------------" << endl << endl;
}

// ---------------------------------------------------------------------------
void Joystick::sendVelocityMsg(float x, float y, float z, float roll, float pitch, float yaw)
{
	// The ROS messages are the same, but topic name has already been configured appropriately in init().
	auv_msgs::BodyVelocityReq rosMsg;
	rosMsg.header.stamp = ros::Time::now();

	// Scale 100% joystick value to SCALE constants.
	rosMsg.twist.linear.x = x * 0.01 * VELOCITY_SCALE_X;
	rosMsg.twist.linear.y = y * 0.01 * VELOCITY_SCALE_Y;
	rosMsg.twist.linear.z = z * 0.01 * VELOCITY_SCALE_Z;
	rosMsg.twist.angular.x = 0;
	rosMsg.twist.angular.y = osl_core::deg2rad(pitch * 0.01 * VELOCITY_SCALE_PITCH);
	rosMsg.twist.angular.z = osl_core::deg2rad(yaw * 0.01 * VELOCITY_SCALE_YAW);

	m_rosVehicleVelPub.publish(rosMsg);

	cout << "Sending velocity request to pilot (m/s, deg/s shown)." << endl;

	const int p = 2;
	const int w = 7;
	cout << endl
			<< "X:    " << fixed << setprecision(p) << setw(w) << rosMsg.twist.linear.x << ", "
			<< "Y:    " << fixed << setprecision(p) << setw(w) << rosMsg.twist.linear.y << ", "
			<< "Z:    " << fixed << setprecision(p) << setw(w) << rosMsg.twist.linear.z << endl
			<< "Roll: " << fixed << setprecision(p) << setw(w) << osl_core::rad2deg(rosMsg.twist.angular.x) << ", "
			<< "Pitch:" << fixed << setprecision(p) << setw(w) << osl_core::rad2deg(rosMsg.twist.angular.y) << ", "
			<< "Yaw:  " << fixed << setprecision(p) << setw(w) << osl_core::rad2deg(rosMsg.twist.angular.z) << endl;

	cout << "--------------- time " << currentTimeString() << " ---------------" << endl << endl;
}
// ---------------------------------------------------------------------------
void Joystick::processNavSts(const auv_msgs::NavStsConstPtr & msg)
{
	m_haveNav = true;
	m_navMsg = msg;
}

// ---------------------------------------------------------------------------
void Joystick::setWaypoint()
{
	m_idWaypoint++;

	m_msgPubWorld.header.stamp = ros::Time::now();
	m_msgPubWorld.goal.requester = "gamepad";
	m_msgPubWorld.goal.id = m_idWaypoint;
	m_msgPubWorld.goal.priority = 1;

	m_msgPubWorld.altitude_mode = false;
	m_msgPubWorld.position.north = m_navMsg->position.north;
	m_msgPubWorld.position.east= m_navMsg->position.east;
	m_msgPubWorld.position.depth = m_navMsg->position.depth;

	m_msgPubWorld.orientation.pitch = 0;		
	m_msgPubWorld.orientation.yaw =m_navMsg->orientation.yaw;

	m_msgPubWorld.disable_axis.x=false;
	m_msgPubWorld.disable_axis.y=false;
	m_msgPubWorld.disable_axis.z=false;
	m_msgPubWorld.disable_axis.pitch=false;
	m_msgPubWorld.disable_axis.yaw=false;

	ROS_INFO_STREAM("Sending: North: "<< m_msgPubWorld.position.north << " East: " << m_msgPubWorld.position.east << " Depth: " << m_msgPubWorld.position.depth << "Pitch: " << m_msgPubWorld.orientation.pitch << " Yaw: " <<  osl_core::rad2deg(m_msgPubWorld.orientation.yaw));
	ROS_INFO_STREAM("Tolerances 0.");

}
// ---------------------------------------------------------------------------
void Joystick::sendWaypoint(){
	m_rosWorldWaypointPub.publish(m_msgPubWorld);
}
// ---------------------------------------------------------------------------
void Joystick::unsetWaypoint()
{
	m_idWaypoint++;

	m_msgPubWorld.header.stamp = ros::Time::now();
	m_msgPubWorld.goal.requester = "gamepad";
	m_msgPubWorld.goal.id = m_idWaypoint;
	m_msgPubWorld.goal.priority = 1;

	m_msgPubWorld.disable_axis.x=true;
	m_msgPubWorld.disable_axis.y=true;
	m_msgPubWorld.disable_axis.z=true;
	m_msgPubWorld.disable_axis.pitch=true;
	m_msgPubWorld.disable_axis.yaw=true;

	m_rosWorldWaypointPub.publish(m_msgPubWorld);

}
// ---------------------------------------------------------------------------
void Joystick::cleanup()
{
	PRINTLN_DEBUG("cleanup entered");

	::close(pad.fd);
}

// ===========================================================================
int main (int argc, char * argv[])
{
	Joystick modCore;
	return modCore.main(argc, argv);
}
